#ifndef HMC5843_h
#define HMC5843_h

#include "../Math/Math.h"

// Rotation matrices
#define ROTATION_NONE				Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
#define ROTATION_YAW_90				Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
#define ROTATION_YAW_180			Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
#define ROTATION_YAW_270			Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)

// orientations for DIYDrones magnetometer
// Components are up!
#define APM_COMPASS_PINS_BACK ROTATION_NONE
#define APM_COMPASS_PINS_RIGHT ROTATION_YAW_90
#define APM_COMPASS_PINS_FORWARD ROTATION_YAW_180
#define APM_COMPASS_PINS_LEFT ROTATION_YAW_270

class HMC5843
{
  private:
	Matrix3f orientationMatrix;
	int offset[3];
	float wrap(float angle);
	void Read();
	void Calculate(float roll, float pitch);
  public:
	float declination;
	float calibration[3];
	int   Mag_X, Mag_Xmin, Mag_Xmax; 
	float Mag_Xzero, Mag_Xrel, Mag_Xspan;
	int   Mag_Y, Mag_Ymin, Mag_Ymax;
	float Mag_Yzero, Mag_Yrel, Mag_Yspan;
	int   Mag_Z, Mag_Zmin, Mag_Zmax;
	float Mag_Zzero, Mag_Zrel, Mag_Zspan;

	float Heading;
	float Heading_X;
	float Heading_Y;
	unsigned long lastUpdate;	
	
	HMC5843();  // Constructor
	void Init(int initialiseWireLib = 1);
	void Update(float roll, float pitch);
	void SetOrientation(const Matrix3f &rotationMatrix);
	void SetDeclination(float radians);
};

#endif
